#ifndef _CONTROL_H
#define _CONTROL_H    1

#include "sk.h"
#include "km.h"
#include "lspb.h"
#include "linear.h"

int checkjoint(double *targetjoint)
{
	for (int i=0; i<6; i++)
	{
		if (fabs(urmes.q_actual[i]-targetjoint[i]) > 0.0001)
		{
			return 1;
		}
		
	}
	return 0;
}

int checkpose(double *targetpose)
{
	for (int i=0; i<6; i++)
	{
		if (fabs(urmes.tcp_actual[i]-targetpose[i]) > 0.0001)
		{
				return 1;
		}
	}

	return 0;
}

void movej(double *joint, double a, double v)
{
	char cmdstr[200];
	sprintf(cmdstr, "movej([%lf,%lf,%lf,%lf,%lf,%lf],a=%lf,v=%lf)\n", joint[0],joint[1],joint[2],joint[3],joint[4],joint[5],a,v);
	sendcmd(cmdstr);
	while(checkjoint(joint));
}

void movel(double *pose, double a, double v)
{
	char cmdstr[200];
	sprintf(cmdstr, "movel(p[%lf,%lf,%lf,%lf,%lf,%lf],a=%lf,v=%lf)\n", pose[0],pose[1],pose[2],pose[3],pose[4],pose[5],a,v);
	sendcmd(cmdstr);
	while(checkpose(pose));
}

void up()
{
	double q[6] = {d2r(0),d2r(-90),d2r(0),d2r(-90),d2r(0),d2r(0)};
	movej(q,1,1);
}

void work()
{
	double q[6] = {d2r(-8.22),d2r(-43.17),d2r(58.69),d2r(-105.49),d2r(-90.43),d2r(0)};
	movej(q,1,1);
}

void home()
{
	double q[6] = {d2r(-33.83),d2r(-117.25),d2r(142.08),d2r(-114.81),d2r(-90.10),d2r(0)};
	movej(q,1,1);
}

void touch()
{
	double p[6];
	for (int i=0; i<6; i++)
	{
		if (i==2)p[i] = urmes.tcp_actual[2] - 0.5*0.001; //每次下降0.5mm
		else p[i] = urmes.tcp_actual[i];
	}
	movel(p,0.1,0.2);
}

int checkjointspeed()
{
	for (int i=0; i<6; i++)
	{
		if (fabs(urmes.qd_actual[i]) != 0)
			return 1;
	}
	return 0;
}

void speedj(double *jointspeed, double acceleration_of_leading_axis, double time)
{
	printf("%lf\n", urmes.qd_actual[1]);
	char cmdstr[200];
	if (time <= 0)
	{
		sprintf(cmdstr, "speedj([%lf,%lf,%lf,%lf,%lf,%lf],a=%lf)\n", jointspeed[0],jointspeed[1],jointspeed[2],jointspeed[3],jointspeed[4],jointspeed[5],acceleration_of_leading_axis);
		printf("%s", cmdstr);
		sendcmd(cmdstr);
	}
	else
	{
		sprintf(cmdstr, "speedj([%lf,%lf,%lf,%lf,%lf,%lf],a=%lf,t=%lf)\n", jointspeed[0],jointspeed[1],jointspeed[2],jointspeed[3],jointspeed[4],jointspeed[5],acceleration_of_leading_axis,time);   
		printf("%s", cmdstr);
		sendcmd(cmdstr);
		usleep(time*1e6+4e5);
	}
}

void moveinj()
{
	FILE *fp = fopen("target.txt", "r");
	int linenum = wc_l("target.txt");
	char tmp[200];
	for (int i=0; i<linenum; i++)
	{
		double q[6];
		for (int j=0; j<6; j++)
		{			
			fscanf(fp, "%s", tmp);
			q[j] = atof(tmp);
		}
		movej(q, 2, 2);
	}
	fclose(fp);
}

void speedinj()
{
	home();
	sleep(1);
	double a = 0.12;
	double t = 2;
	double qd[6] = {0,-0.2,0,0,0,0};
	speedj(qd,-a,t);
	printf("%lf\n", urmes.qd_actual[1]);
	// speedj(qd,a,t);
	// usleep(1e5);
	// while(1) {
	//     if (urmes.qd_actual[1] == 0) break;
	//     printf("%lf\n", urmes.qd_actual[1]);
	//     usleep(1e4);
	// }
}

void help()
{
	print("\n----help----\n");
	print("exit  退出程序");
	print("home  移动至home位置");
	print("\n----eofh----\n");
}

void moveltest()
{
	char cmds[20000] = "def move():\n";
	
	FILE *rfp = fopen("automatic_targetPose_interpolated_afterIK.txt", "r");
	int lines = wc_l("automatic_targetPose_interpolated_afterIK.txt");
	double q[6];
	double r = 0.15*1e-3; //blend_radius < 0.5*min(dist(i,j), dist(j,k))
	char buf[100];
	for (int i=0; i<lines; i++)
	{
		for (int j=0; j<6; j++)
		{
			fscanf(rfp, "%s", buf);
			q[j] = atof(buf);
		}
		sprintf(cmds+strlen(cmds), "\tmovel([%lf,%lf,%lf,%lf,%lf,%lf],a=1,v=0.2,t=0,r=%lf)\n", q[0],q[1],q[2],q[3],q[4],q[5],r);
	}
	fclose(rfp);

	sprintf(cmds+strlen(cmds), "end\n");
	sendcmd(cmds);
}

/* 单关节的多段带有过渡区段的直线路径规划 */
void straight()
{
	/* 得到轨迹起始点 */
	double start_point[6];

	FILE *ifp = fopen("关节1轨迹.txt", "r");
	char buf[100];
	for (int i=0; i<6; i++)
	{
		fscanf(ifp, "%s", buf);
		start_point[i] = d2r(atof(buf));
	}
	fclose(ifp);

	/* 移动至起始点 */
	movej(start_point, 1, 2);
	
	print("已到达起始点，任意键继续");
	getchar();

	char cmds[20000];

	/* 得到规划路径 */
	StraightPath(cmds, 1, 1);

	sendcmd(cmds);
}

/* 多关节的多段带有过渡区段的直线路径规划 */
void linear()
{
	/* 得到轨迹起始点 */
	double start_point[6];

	FILE *ifp = fopen("多轴的直线路径轨迹.txt", "r");
	char buf[100];
	for (int i=0; i<6; i++)
	{
		fscanf(ifp, "%s", buf);
		start_point[i] = atof(buf);
	}
	fclose(ifp);

	/* 移动至起始点 */
	movej(start_point, 1, 2);
	
	print("已到达起始点，任意键继续");
	getchar();

	char cmds[20000];

	/* 得到规划路径 */
	StraightPath_MultAxis(cmds, 1, 5);

	sendcmd(cmds);
}

void draw()
{
	work(); //先进入工作状态
	char fname[20] = "write.txt";
	int line = wc_l(fname);
	FILE *fp = fopen(fname, "r");
	if (fp == NULL)
	{
		printf("error to open %s\n", fname);
		exit(0);
	}
	double xstart = urmes.tcp_actual[0];
	double ystart = urmes.tcp_actual[1];
	printf("xstart: %lf\n", xstart*1000);
	printf("ystart: %lf\n", ystart*1000);
	char buf[1024] = {0};
	for (int j=0; j<line; j++)
	{
		fscanf(fp, "%s", buf);
		int num = atoi(buf);
		for (int i=0; i<num; i++)
		{
			g_record = 1;

			double x, y;
			fscanf(fp, "%s", buf); x = atof(buf);
			fscanf(fp, "%s", buf); y = atof(buf);
			if (i == 0)
			{
				double tmp1[6] = {xstart+x*0.001,ystart+y*0.001,(HEIGHT+2)*0.001,urmes.tcp_actual[3],urmes.tcp_actual[4],urmes.tcp_actual[5]};
				movel(tmp1,0.2,0.4);
				double tmp2[6] = {xstart+x*0.001,ystart+y*0.001,HEIGHT*0.001,urmes.tcp_actual[3],urmes.tcp_actual[4],urmes.tcp_actual[5]};
				movel(tmp2,0.2,0.4);
			}
			else
			{
				double tmp1[6] = {xstart+x*0.001,ystart+y*0.001,HEIGHT*0.001,urmes.tcp_actual[3],urmes.tcp_actual[4],urmes.tcp_actual[5]};
				movel(tmp1,0.2,0.4);
				if (i == num - 1)
				{
					double tmp2[6] = {xstart+x*0.001,ystart+y*0.001,(HEIGHT+2)*0.001,urmes.tcp_actual[3],urmes.tcp_actual[4],urmes.tcp_actual[5]};
					movel(tmp2,0.2,0.4);
				}
			}

			g_record = 0;
		}
	}
	fclose(fp);
	work(); //复位
}

void status()
{
	while (1) {
		printf("%lf %lf %lf %lf %lf %lf\n", urmes.tcp_actual[0],urmes.tcp_actual[1],urmes.tcp_actual[2],urmes.tcp_actual[3],urmes.tcp_actual[4],urmes.tcp_actual[5]);
	}
}

void lspb()
{
	/* 得到轨迹起始点 */
	double start_point[6];

	FILE *ifp = fopen("多轴的直线路径轨迹.txt", "r");
	char buf[100];
	for (int i=0; i<6; i++)
	{
		fscanf(ifp, "%s", buf);
		start_point[i] = atof(buf);
	}
	fclose(ifp);

	/* 移动至起始点 */
	movej(start_point, 1, 2);
	
	print("已到达起始点，任意键继续");
	getchar();

	char cmds[20000];

	/* 得到规划路径 */
	Normal_LSPB(cmds);

	print(cmds);
	// sendcmd(cmds);
}

void converse()
{
	while (1) {
		char sentence[200];
		fgets(sentence, sizeof sentence / sizeof sentence[0], stdin);
		printf("%s", sentence);
		if (strcmp(sentence, "exit\n") == 0)
		{
			break;
		}
		strcat(sentence, "\n");
		sendcmd(sentence);
	}
}

void movejbystep()
{
	int LineNum = wc_l("关节1轨迹.txt");

	double data[LineNum*6];

	ReadPath("关节1轨迹.txt", LineNum, data);

	FILE *ofp = fopen("关节1轨迹_笛卡尔空间.txt", "w");

	for (int i=0; i<LineNum; i++)
	{
		movej(data+i*6, 1, 2);
		print("任意键继续");
		getchar();
		fprintf(ofp, "%lf %lf %lf %lf %lf %lf\n", urmes.tcp_actual[0],urmes.tcp_actual[1],urmes.tcp_actual[2],urmes.tcp_actual[3],urmes.tcp_actual[4],urmes.tcp_actual[5]);
	}

	fclose(ofp);
}

void writepath()
{
	FILE *fp1 = fopen("多轴的直线路径轨迹.txt", "w");
	FILE *fp2 = fopen("多轴的直线路径轨迹_笛卡尔空间.txt", "w");

	int i = 8;
	while(--i)
	{
		print("任意键继续");
		getchar();
		fprintf(fp1, "%lf %lf %lf %lf %lf %lf\n", urmes.q_actual[0],urmes.q_actual[1],urmes.q_actual[2],urmes.q_actual[3],urmes.q_actual[4],urmes.q_actual[5]);
		fprintf(fp2, "%lf %lf %lf %lf %lf %lf\n", urmes.tcp_actual[0],urmes.tcp_actual[1],urmes.tcp_actual[2],urmes.tcp_actual[3],urmes.tcp_actual[4],urmes.tcp_actual[5]);
	} 

	fclose(fp1);
	fclose(fp2);
}

/* 得到d1, a2, a3, d4, d5, d6 */
void dh()
{
	double dh[6];

	sendcmd("movej([0, 0, 0, d2r(-90), 0, 0], a = 2, v = 2)\n");
	sleep(1);
	while(checkjointspeed());
	dh[0] = fabs(urmes.tcp_actual[2]);

	sendcmd("movej([0, 0, 0, 0, d2r(90), 0], a = 2, v = 2)\n");
	sleep(1);
	while(checkjointspeed());
	dh[3] = fabs(urmes.tcp_actual[1]);

	sendcmd("movej([0, 0, d2r(-90), 0, 0, 0], a = 2, v = 2)\n");
	sleep(1);
	while(checkjointspeed());
	dh[2] = fabs(fabs(urmes.tcp_actual[2]) - dh[0]);

	sendcmd("movej([0, 0, 0, 0, 0, 0], a = 2, v = 2)\n");
	sleep(1);
	while(checkjointspeed());
	dh[1] = fabs(fabs(urmes.tcp_actual[0]) - dh[2]);
	dh[5] = fabs(fabs(urmes.tcp_actual[1]) - dh[3]);
	dh[4] = fabs(dh[0] - fabs(urmes.tcp_actual[2]));

	printf("%lf %lf %lf %lf %lf %lf\n", dh[0],dh[1],dh[2],dh[3],dh[4],dh[5]);
}

void nofound()
{
	print("\n找不到该命令，请输入help\n");
}

void control()
{
	// while (1) {
		printf("Enter command to exec: ");
		char buf[100];
		fgets(buf, sizeof(buf), stdin);
		if      (cmdis("exit\n"))              exit(0);
		else if (cmdis("help\n"))              help();
		else if (cmdis("home\n"))              home();
		else if (cmdis("up\n"))                up();
		else if (cmdis("work\n"))              work();
		else if (cmdis("touch\n"))             touch();
		else if (cmdis("draw\n"))              draw();
		else if (cmdis("speedinj\n"))          speedinj();
		else if (cmdis("iktest\n"))            iktest();
		else if (cmdis("moveinj\n"))           moveinj();
		else if (cmdis("moveltest\n"))         moveltest();
		else if (cmdis("straight\n"))          straight();
		else if (cmdis("status\n"))            status();
		else if (cmdis("findOptimalSolu\n"))   find();
		else if (cmdis("lspb\n"))              lspb();
		else if (cmdis("converse\n"))          converse();
		else if (cmdis("movejbystep\n"))       movejbystep();
		else if (cmdis("writepath\n"))         writepath();
		else if (cmdis("linear\n"))            linear();
		else if (cmdis("dh\n"))                dh();
		else nofound();
	// } /* comment while(1) */
}

#endif /* <control.h> included.  */